#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 将gazebo的定位数据转为pose 2021年1月8日
import os
import time
import numpy as np
import math
import rospy
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
from std_msgs.msg import Int32
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
import time

car1_set_pose = [-1.8, -1.5, 0]
car2_set_pose = [-1.8, -0.5, 0]
car3_set_pose = [-1.8, 0.5, 0]
car4_set_pose = [-1.8, 1.5, 0]
set_angle = [0, 0, 0 * math.pi / 180]
f = 2.0

class gazebo_reset:
    def __init__(self):
        # 设置循环的频率
        rate = rospy.Rate(f)
        rospy.Subscriber('/reset', Int32, self.reset_callback)
        # Main while loop.
        global reset_flag
        reset_flag = 0
        while not rospy.is_shutdown():
            if reset_flag:
                self.reset()
                reset_flag = reset_flag - 1

            rate.sleep()

    def reset(self):
        # Reset
        print('Reset')
        car1_state.pose.position = self.set_pose(car1_set_pose)
        car1_state.pose.orientation = self.euler_to_quat(set_angle)
        car2_state.pose.position = self.set_pose(car2_set_pose)
        car2_state.pose.orientation = self.euler_to_quat(set_angle)
        car3_state.pose.position = self.set_pose(car3_set_pose)
        car3_state.pose.orientation = self.euler_to_quat(set_angle)
        car4_state.pose.position = self.set_pose(car4_set_pose)
        car4_state.pose.orientation = self.euler_to_quat(set_angle)

        car1_state.twist = Twist()
        car2_state.twist = Twist()
        car3_state.twist = Twist()
        car4_state.twist = Twist()

        pose_pub.publish(car1_state)
        pose_pub.publish(car2_state)
        pose_pub.publish(car3_state)
        pose_pub.publish(car4_state)

        time.sleep(0.2)

    def reset_callback(self, msg):
        global reset_flag
        if msg.data == 1:
            reset_flag = 3
        print('Receive reset_flag  ', msg.data)

    def set_pose(self, pose):
        [x, y, z] = pose
        point = Point()
        point.x = x
        point.y = y
        point.z = z
        return point

    def euler_to_quat(self, rqy):
        [r, p, y] = rqy
        quaternion = Quaternion()
        sinp = math.sin(p / 2)
        siny = math.sin(y / 2)
        sinr = math.sin(r / 2)

        cosp = math.cos(p / 2)
        cosy = math.cos(y / 2)
        cosr = math.cos(r / 2)

        quaternion.w = cosr * cosp * cosy + sinr * sinp * siny
        quaternion.x = sinr * cosp * cosy - cosr * sinp * siny
        quaternion.y = cosr * sinp * cosy + sinr * cosp * siny
        quaternion.z = cosr * cosp * siny - sinr * sinp * cosy
        return quaternion

if __name__ == '__main__':
    # ROS节点初始化
    rospy.init_node('gazebo_reset', anonymous=True)
    try:
        car1_vel_pub = rospy.Publisher('/car1/cmd_vel', Twist, queue_size=10)
        car2_vel_pub = rospy.Publisher('/car2/cmd_vel', Twist, queue_size=10)
        car3_vel_pub = rospy.Publisher('/car3/cmd_vel', Twist, queue_size=10)
        car4_vel_pub = rospy.Publisher('/car4/cmd_vel', Twist, queue_size=10)
        pose_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
        reset_pub = rospy.Publisher('/reset', Int32, queue_size=10)
        car1_state, car2_state, car3_state, car4_state = ModelState(), ModelState(), ModelState(), ModelState()
        car1_state.model_name = 'car1'
        car2_state.model_name = 'car2'
        car3_state.model_name = 'car3'
        car4_state.model_name = 'car4'
        gazebo_reset()
    except rospy.ROSInterruptException:
        pass
